#include <cmath>

#include "heading.h"

namespace perception
{
    namespace algorithm
    {

        Heading::Heading()
        {
        }
        Heading::~Heading()
        {
        }

        float Heading::Compute(std::vector<data_cell> &lidar_objects)
        {
            // 1.转换坐标，对每个点进行航向的坐标转换

            // 2.计算当前的航向所对应的方差，取最小的最为航向
            // return GetBestRad(lidar_objects);
            return GetHeading(lidar_objects);
        }
        float Heading::GetBestRad(std::vector<data_cell> &cells)
        {
            rad = 0;
            float var = 1000;
            float best_rad = rad;
            for (size_t i = 0; i < 180 / drad; i++)
            {
                // 去均值化
                float rad_tmp = rad;
                auto tmp = cells;
                data_cell mean;
                ComputeMean(tmp, mean);
                CoordinateTransformation(tmp, rad_tmp);

                auto var_ = ComputeVariance(tmp, mean);
                if (var_ < var)
                {
                    best_rad = rad_tmp;
                    var = var_;
                }
                rad = rad + drad;
            }
            return best_rad;
        }
        float Heading::ComputeVariance(std::vector<data_cell> &cells, data_cell &mean)
        {
            float var = 0;
            for (size_t i = 0; i < cells.size(); i++)
            {
                // float a = std::pow(cells[i].x - mean.x, 2) + std::pow(cells[i].y - mean.y, 2);
                // float a = std::pow(cells[i].y - mean.y, 2);
                float a = std::pow(cells[i].y, 2);
                var += a;
            }
            return var;
        }
        void Heading::CoordinateTransformation(std::vector<data_cell> &cells, float &rad)
        {
            auto tmp = cells;
            for (size_t i = 0; i < cells.size(); i++)
            {
                cells[i].x = tmp[i].x * std::cos(rad) + tmp[i].y * std::sin(rad);
                cells[i].y = tmp[i].y * std::cos(rad) - tmp[i].x * std::sin(rad);
            }
        }
        // 本来是求均值,但不对,改为求几何中心
        void Heading::ComputeMean(std::vector<data_cell> &cells, data_cell &mean)
        {
            // float sum_x = 0, sum_y = 0;
            // for (size_t i = 0; i < cells.size(); i++)
            // {
            //     sum_x += cells[i].x;
            //     sum_y += cells[i].y;
            // }
            // mean.x = sum_x / cells.size();
            // mean.y = sum_y / cells.size();
            float min_x = std::numeric_limits<float>::max();
            float min_z = std::numeric_limits<float>::max();
            float min_y = std::numeric_limits<float>::max();
            float max_x = -std::numeric_limits<float>::max();
            float max_y = -std::numeric_limits<float>::max();
            float max_z = -std::numeric_limits<float>::max();
            for (auto &cell : cells)
            {

                min_x = std::min(min_x, cell.x);
                max_x = std::max(max_x, cell.x);
                min_y = std::min(min_y, cell.y);
                max_y = std::max(max_y, cell.y);
                min_z = std::min(min_z, cell.z_min);
                max_z = std::max(max_z, cell.z_max);
            }
            mean.x = (max_x + min_x) / 2;
            mean.y = (max_y + min_y) / 2;
            // mean.z = (max_z + min_z) / 2;
            for (auto &cell : cells)
            {
                cell.x = cell.x - mean.x;
                cell.y = cell.y - mean.y;
                // std::cout << "cell.x: " << cell.x << std::endl;
                // std::cout << "mean.x: " << mean.x << std::endl;
            }
        }
        // 前方目标： 找出y最大和最小的点，x最小的点，max_y->min_x 和min_y->min_x向量，夹角在90+-10范围的，直接
        //          比较两个向量长度，长的为航向，大于范围的比较长宽，长比宽长，航向0，反之航向90，且长宽互换
        float Heading::GetHeading(std::vector<data_cell> &cells)
        {
            // 1.求最大最小点
            data_cell min_y;
            data_cell max_y;
            data_cell min_x;
            data_cell max_x;
            min_y.y = 100;
            max_y.y = -100;
            min_x.x = 100;
            max_x.x = -100;
            for (auto &cell : cells)
            {
                if (cell.x < min_x.x)
                    min_x = cell;
                if (cell.x > max_x.x)
                    max_x = cell;
                if (cell.y < min_y.y)
                    min_y = cell;
                if (cell.y > max_y.y)
                    max_y = cell;
            }
            //
            float heading = 0;
            if (min_x.x > 0)
            {
                float maxy_x = std::sqrt(std::pow(max_y.x - min_x.x, 2) + std::pow(max_y.y - min_x.y, 2));
                float miny_x = std::sqrt(std::pow(min_y.x - min_x.x, 2) + std::pow(min_y.y - min_x.y, 2));

                float maxy_miny = std::acos(((max_y.x - min_x.x) * (min_y.x - min_x.x) +
                                              (max_y.y - min_x.y) * (min_y.y - min_x.y)) /
                                             maxy_x * miny_x) *
                                  90 / M_PI_2;
                if (maxy_miny < 60 || maxy_miny > 120)
                {
                    std::cout << "maxy_miny:" << maxy_miny << std::endl;
                    std::cout << "max_y:" << max_y.x <<" " << max_y.y <<std::endl;
                    std::cout << "min_y:" << min_y.x <<" " << min_y.y <<std::endl;
                    std::cout << "min_x:" << min_x.x <<" " << min_x.y <<std::endl;
                    heading = 0;
                    return heading;
                }

                if (maxy_x > miny_x)
                    heading = std::atan2(max_y.y - min_x.y, max_y.x - min_x.x);
                else
                    heading = std::atan2(min_y.y - min_x.y, min_y.x - min_x.x);
                std::cout << "heading:" << heading << std::endl;
            }
            else if (max_x.x < 0)
            {
                float maxy_x = std::sqrt(std::pow(max_y.x - max_x.x, 2) + std::pow(max_y.y - max_x.y, 2));
                float miny_x = std::sqrt(std::pow(min_y.x - max_x.x, 2) + std::pow(min_y.y - max_x.y, 2));

                if (maxy_x > miny_x)
                    heading = std::atan2(max_y.y - max_x.y, max_y.x - max_x.x);
                else
                    heading = std::atan2(min_y.y - max_x.y, min_y.x - max_x.x);
            }
            else
            {
            }
            return heading;
        }
        // namespace perception

    } // namespace perception
} // namespace avos